#include "blpf2.h"

/**
 * @brief  滤波器初始化
 * @param  hblpf2:巴特沃斯滤波器结构体
 * @param  wc:截至频率
 * @param  dt:离散间隔时间
 * @retval void
 */
void blpf2_init(blpf2_t *hblpf2, float wc, float dt)
{
    /* 滤波次数 */
    hblpf2->apply_num = 0;

    /* 合法性检查 */
    if (dt == 0) {
        warning("间隔时间不能为0");
    }
    
    if (hblpf2 == 0) {
        warning("非法的结构体指针");
    }

    /* f = 1 / t */
    float fd = 1.0f / dt;

    /* ohm = tan(pi*fd / fs)*/
    float ohm = tanf(LPF_PI * wc / fd);

    /* c = 1 + 1.414*ohm + ohm² */
    float c = 1.0f + (1.414f * ohm) + (ohm * ohm);

    /* b0 = ohm² / c */
    hblpf2->b0 = ohm * ohm / c;

    /* b1 = 2 * ohm² / c */
    hblpf2->b1 = 2.0f * hblpf2->b0;

    /* b2 = ohm² / c*/
    hblpf2->b2 = hblpf2->b0;

    /* a1 = 2 * ohm² - 2 / c */
    hblpf2->a1 = ((2.0f * ohm * ohm) - 2.0f) / c;

    /* a2 = 1 - 1.414*ohm + ohm² / c*/
    hblpf2->a2 = (1.0f - (1.414f * ohm) + (ohm * ohm)) / c;
}

/**
 * @brief  应用滤波器
 * @param  in:输入
 * @retval 滤波后的值
 */
float blpf2_apply(blpf2_t *hblpf2, float xn)
{
    hblpf2->xn[0] = xn;

    /* Y(n) = b0*X(n) + b1*X(n-1) + b1*X(n-2) + a0*Y(n-1) + a1*Y(n-2) */
    hblpf2->yn[0] = hblpf2->b0 * hblpf2->xn[0] + hblpf2->b1 * hblpf2->xn[1] + hblpf2->b2 * hblpf2->xn[2] -\
                    hblpf2->a1 * hblpf2->yn[1] - hblpf2->a2 * hblpf2->yn[2]; 
    
    /* x序列更新 */
    hblpf2->xn[2] = hblpf2->xn[1];
    hblpf2->xn[1] = hblpf2->xn[0];

    /* y序列更新 */
    hblpf2->yn[2] = hblpf2->yn[1];
    hblpf2->yn[1] = hblpf2->yn[0];

    /* 等待运行三个周期 */
    if ( hblpf2->apply_num < 3) {
        hblpf2->apply_num++;
        return xn;
    }
    
    /* 判断输出有界性 */
    if (!isfinite(hblpf2->yn[0])) {
        return xn;
    }
    
    return hblpf2->yn[0];
}

/******************* (C) COPYRIGHT 2024 FUYEBING*****END OF FILE************/